COMMENT ⊗   VALID 00011 PAGES 
RECORD PAGE   DESCRIPTION
 00001 00001
 00003 00002	ac assignments
 00004 00003	↓CWORD:	BLOCK 14	contains joint control words
 00006 00004		BEGIN SERVO
 00007 00005	We run through this loop 14 times, once for each hand and once for each joint
 00011 00006	perform the servo calculations
 00015 00007	we now convert the torque in T into a motor drive.
 00018 00008	check if we need to switch wipers
 00021 00009	we now compute the joint angles for the next time
 00025 00010	This routine reads the real time clock and then computes the change in time
 00029 00011	takes motor torque in ac1 and drives motor (JOINT) if o.k.
 00032 ENDMK
⊗;
;ac assignments

	↓AC1←1		;used for passing first arg to subroutines and for return value
	↓AC2←2		;used for passing second arg to subroutines
	↓AC3←3		;used for passing third arg to subroutines

;acs 1 through 3 must be saved by routines if they do not contain args

	↓TAC1←4		;tempoary ac does not have to be saved at any level
	↓TAC2←5		;tempoary ac does not have to be saved at any level
	↓TAC3←6		;tempoary ac does not have to be saved at any level

	↓P←←17		;push down pointer
	↓Q←16		;jsp pointer

↓CWORD:	BLOCK 14	;contains joint control words

;is this joint running?
	RUN←←1		;if RUN is off then the following flags are all zero
			;and have no meaning.

;what state is it in?
	FIRST←←2	;first time through loop for this motion.
	FINAL←←4	;in final state, nulling errors
	STOP←←10000	;stop this joint

;how is it running?
	SERVO←←10	;position servo
	VELS←←20	;velocity servo
	FORCE←←40	;exert force
	PDIR1←←100	;with this prefered direction of motion
	PDIR2←←200	;01 up, 00 stop, 10 down
	WOB←←400	;perturb this joint while running

;how will the motion be stopped
	NUL←←1000	;null errors at end and stop
	SOF←←2000	;stop on force
	SOT←←4000	;stop on touch

↓LTIME:	BLOCK 14	;last time this joint was serviced
↓CHAN:	BLOCK 14	;the current a/d channel joint
↓OFF:	BLOCK 14	;contains the current offset for this joint

	BEGIN SERVO

;ac's
	JOINT←15	;which joint is being servoed
	CONTROL←14	;joint control word
	TRY←13		;number of times to try servoing a joint each jiffy
	T←12		;time in micro secs into segment

ADCNT:	0		;number of times to try to read a/d
BDELTA:	BLOCK 14	;time diff between beginning of service and last service
;We run through this loop 14 times, once for each hand and once for each joint

	MOVEI JOINT,=13			;which joint we are on
	JFCL 10,.+1			;turn off overflow flag

↑SERVO:	MOVE CONTROL,CWORD(JOINT)	;load control word
	TLNN CONTROL,RUN		;is this joint running?
	JRST DTEST

	MOVEI TRY,4			;number of times to try reading a/d

	MOVE AC1,LTIME(JOINT)		;last time we servoed this joint
	JSP Q,PERIOD			;read change in time
	MOVEM AC1,BDELTA(JOINT)		;change in time from last servo
	SUB AC1,PDELTA(JOINT)		;predicted delta ac1=delta-pred
	FMPR AC1,[-0.000006]		;delta delta t
	MOVEM AC1,DELDEL

REAPOS:	MOVE AC1,CHAN(JOINT)		;a/d channel for this joint
	JSP Q,PREAD			;get the a/d reading
	JSP Q,AVE			;compute the average and skip 2 if o.k.
	SOJGE TRY,REAPOS		;readings too noisy
	JRST[	MOVEI AC1,NOIERR
		MOVEM AC1,TRAJER
		JSP Q,DHO]

;now we do the non-linear calibration

	MUL AC1,REFTR			;multiply by the reference voltage factor
	MOVEI TAC1,AC1
	ASHC TAC1,-11			;leave index into non-linear diff table in
					;tac1 and the fraction in tac2
	LDB AC2,NON(JOINT)		;lower diff
	ADDI AC1,(AC2)			;add it to the reading
	AOJ TAC1,
	LDB AC3,NON(JOINT)		;upper diff
	SUBI AC3,(AC2)
	MUL TAC2,AC3			;interpolate
	ADD AC1,TAC2
	MOVEM AC1,POT(JOINT)		;used to determine when to switch wipers.
	FSC AC1,226
	FMPR AC1,SCALE(JOINT)		;multiply by the scale factor
	FADR AC1,@OFF(JOINT)		;add the offset
	MOVEM AC1,TH(JOINT)		;observed theta

	TLNE CONTROL,STOP		;stop this joint?
	JRST EVAL			;fix set point

;now measure the joint velocity

REAVEL:	SKIPN AC1,TACH(JOINT)		;use tach if there is one
	JRST[	MOVE AC1,TH(JOINT)	;no tach so compute vel by differencing
		FSBR AC1,THP(JOINT)	;last reading
		MOVE TAC1,BDELTA(JOINT)
		FSC TAC1,233
		FMPR TAC1,[0.00006]	;1/16,667
		FDVR AC1,TAC1		;velocity
		JRST STVEL]
	JSP Q,PREAD			;read the tach.
	JSP Q,AVE			;average the readings and skip 2 if o.k.
	SOJGE TRY,REAVEL		;readings too noisy
	JRST[	MOVEI AC1,NOIERR
		MOVEM AC1,TRAJER
		JSP Q,DHO]
	TRNE CONTROL,FIRST			;first time through
	MOVEM AC1,VZERO(JOINT)		;store zero reading
	SUB AC1,VZERO(JOINT)
	FSC AC1,216
	FMPR AC1,VSCALE			;scale
STVEL:	MOVEM AC1,TD(JOINT)		;store velocity
;perform the servo calculations

	SETZM ET0(JOINT)		;initialize position error to zero
	TLNE CONTROL,FORCE		;just exert force?
	JRST[				;yes
		TLNN CONTROL,PDIR1+PDIR2;some prefered direction of motion if
					;the joint is stopped?
		JRST DRIVE		;no.
		TLNE CONTROL,PDIR1	;up?
		MOVE AC1,ERR(JOINT)	;direction of intended motion
		TLNE CONTROL,PDIR2	;down?
		MOVN AC1,ERR(JOINT)	;direction of intended motion
		MOVEM AC1,ET0(JOINT)	;position error
		JRST DRIVE]		;just output drive now.

	TLNE CONTROL,VSER		;if velocity servo
	JRST[				;zero ac1 and compute velocity feedback
		SETZ AC1,
		JRST VBACK]

	MOVE AC1,TDP(JOINT)		;predicted velocity
	FMPR AC1,DELDEL
	FADR AC1,TH(JOINT)		;otherwise compute position error
	FSBR AC1,T0(JOINT)

	MOVEM AC1,ET0(JOINT)		;position error

	TRNE FINAL			;nulling errors?
	JRST[
		FADRM AC1,ERRINT(JOINT)	;integrate error
		MOVM AC2,AC1		;|error|
		CAMLE AC2,ERR(JOINT)	;less than tolerance?
		JRST .+1		;no keep going
		MOVM AC2,TD(JOINT)	;|vel|
		CAMLE AC2,ERR(JOINT)	;less than error tolerance?
		JRST .+1		;no keep on

		MOVEI AC1,(JOINT)
		JSP Q,STOPJT		;stop this joint
		MOVE AC1,LTIME(JOINT)	;last time we servoed this joint
		JSP Q,PERIOD		;read change in time
		SUB AC1,BDELTA(JOINT)	;change in time from beginning of loop
		CAIL AC1,=500		;less than 1/2 milli second
		JRST[
			SOJGE TRY,SERVO
			MOVEI AC1,TOLERR
			MOVEM AC1,TRAJER
			JSP Q,DHO]
		TLO CONTROL,STOP	;turn on stop bit
		JRST DTEST]		;do next joint

	FMPR AC1,KE(JOINT)		;ke * position error
	MOVN AC2,ERRINT(JOINT)
	FMPR AC2,KI(JOINT)		;-ki * integral error
	FSBR AC1,AC2			;-ke * pos err -ki * int err
VBACK:	MOVE AC2,TDD(JOINT)		;predicted acceleration
	FMPR AC2,DELDEL
	FADR AC2,TD(JOINT)		;velocity
	FSBR AC2,TDP(JOINT)		;-vel error
	FMPR AC2,KV(JOINT)		;-kv * vel error
	FSBR AC1,AC2
	MOVEM AC1,TORE(JOINT)		;error correcting torque
	FADR AC1,TP(JOINT)		;preloaded with T(G) and J*TDD(I)
	MOVEM AC1,T(JOINT)		;the required joint torque

;we now convert the torque in T into a motor drive.
DRIVE:
	MOVE AC1,TD(JOINT)			;joint velocity
	JUMPN AC1,.+2				;skip if non-zero
	MOVN AC1,ET0(JOINT)			;intended direction of motion
	FMPR AC1,T(JOINT)			;dot it with reqd. torque
	JUMPL AC1,[				;motion opposed to force
		MOVM AC1,T(JOINT)		;|T - F0|,F0 is joint static friction
		FSBR AC1,F0(JOINT)
		JUMPL AC1,[			;less than F0
			FDVR AC1,F0(JOINT)	;D=(2*D0*(T-F0)/F0)+D0
			FMPR AC1,D0(JOINT)
			FSC AC1,1
			FADR AC1,D0(JOINT)
			SKIPGE T(JOINT)		;if T<0 then negate
			MOVN AC1,AC1
			JRST CDM]
		MOVSI AC2,(1.0)			;greater then F0
		FSBR AC2,PK(JOINT)		;1-mu
		FMPR AC1,AC2			;|T*(1-mu)|
		FADR AC1,D0(JOINT)		;+D0
		SKIPG T(JOINT)			;if T<0 then negate
		MOVN AC1,AC1
		JRST CDM]
	SKIPN T(JOINT)				;motion with force
	JRST[
		SKIPN TD(JOINT)			;but no T reqd.
		JRST CDM			;and if stopped then thats it
		MOVE AC1,D0(JOINT)		;otherwise put out D0 in the 
		SKIPGE TD(JOINT)		;direction its going.
		MOVN AC1,AC1
		JRST CDM]
	JUMPE AC1,NOEX				;T but no vel or pref. dir.
	MOVE AC1,D0(JOINT)			;D=D0+T*(1+mu)
	SKIPGE T(JOINT)
	MOVN AC1,AC1				;load V0 and negate if necc.
NOEX:	MOVSI AC2,(1.0)
	FADR AC2,PK(JOINT)
	FMPR AC2,T(JOINT)
	FADR AC1,AC2				;add V0
CDM:	JSP Q,MOTOR				;convert torque to motor drive

TTEST:	MOVE AC1,LTIME(JOINT)			;last time we servoed this joint
	JSP Q,PERIOD				;read change in time
	SUB AC1,BDELTA(JOINT)			;change in time from beginning of loop
	CAIL AC1,=1000				;less than 1 milli second
	JRST[
		SOJGE TRY,SERVO			;try to servo this joint faster.
		MOVEI AC1,TOLERR
		MOVEM AC1,TRAJER
		JSP Q,DHO]
;check if we need to switch wipers
	SKIPE MAXCHA(JOINT)				;if zero only one wiper
	JRST[						;check if we need to switch?
		MOVE AC1,POT(JOINT)			;octal pot reading
		CAIGE AC1,16200				;skip if greater than 8/9 ths.
		CAIG AC1,1600				;do not skip if less than 1/9 th.
		JRST[					;switch wipers!
			CAIL AC1,16200			;too high?
			JRST[
				SOSGE TAC1,INDEX(JOINT)	;decrease index
				JRST[			;below zero
					ADDI TAC1,1(MAXCHA);select top channel
					MOVEM TAC1,INDEX(JOINT)
					MOVE TAC1,MAXCHA
					MOVE AC3,[360.0]
				ADDTST:	FADRM AC3,@OFF(JOINT);add 360 to all offsets
					SOJGE TAC1,ADDTST;offset indexed by tac1 indirectly
					JRST WIPROK]
				JRST WIPROK]
			AOS TAC1,INDEX(JOINT)		;too low increase index
			CAMLE TAC1,MAXCHA(JOINT)	;above top channel?
			JRST[
				SETZM INDEX(JOINT);select bottom channel
				MOVE TAC1,MAXCHA
				MOVE AC3,[-360.0]
			SUBTST:	FADRM AC3,@OFF(JOINT);add -360 to all offsets
				SOJGE TAC1,SUBTST;offset indexed by tac1 indirectly
				JRST WIPROK]
			JRST WIPROK]
		JRST WIPROK]
WIPROK:
;now check the limits of joint motion
	MOVE AC1,TH(JOINT)				;joint angle
	CAMGE AC1,MAXANG(JOINT)				;skip if > max angle
	CAMG AC1,MINANG(JOINT)				;do not skip if < min angle
	JRST[						;pot out of range
		MOVEI AC1,JRNERR
		MOVEM AC1,TRAJER
		JSP Q,DHO]
;all is now fine in the world of pots.
;we now compute the joint angles for the next time
EVAL:
	MOVE AC3,TP(JOINT)				;points to coefficients
	JUMPE AC3,NE					;at end of trajectory
	MOVE T,BDELTA(JOINT)				;elapsed time this segment
	ADDB T,TJ(JOINT)				;predicted time for next service
	CAML T,TN(JOINT);				;time in micro sec this segment
	JRST[						;end of segment
		LDB AC3,[POINT 9,(AC3),17]		;next pointer
		JUMPE AC3,[SETZM TP(JOINT)		;end of the run
			TLNN CONTROL,NUL		;skip if null errors
			JRST[	JSP Q,STOPJT		;stop the joint
				TLO CONTROL,STOP	;flag it
				JRST SJT]
			TLO CONTROL,FINAL		;set final to null errors
		SJT:	MOVE AC1,A5(JOINT)
			ADD AC1,A4(JOINT)
			ADD AC1,A3(JOINT)
			ADD AC1,A2(JOINT)
			ADD AC1,A1(JOINT)
			ADD AC1,A0(JOINT)
			XOR AC1,SC(JOINT)
			FSC AC1,0
			MOVEM AC1,T0(JOINT)
			SETZM TDP(JOINT)
			SETZM TDD(JOINT)
			JRST NE]
	GOT:	ADDI AC3,(BASE)
		MOVEM AC3,TP(JOINT)
		SUB T,TN(JOINT);MICRO SEC INTO NEW SEGMENT
		MOVEM T,TJ(JOINT)	;INITIALIZE
		HRRZ AC1,(AC3)
		FIX AC1,251
		MOVEM AC1,TN(JOINT)
		SETZM A5(JOINT)
		SETZM A55(JOINT)
		SETZM A520(JOINT)
		HLRE AC1,-3(AC3)
		MOVEM AC1,A4(JOINT)
		FSC AC1,2
		MOVEM AC1,A44(JOINT)
		FMPR AC1,[3.0]
		MOVEM AC1,A412(JOINT)
		HRRE AC1,-3(AC3)
		MOVEM AC1,A3(JOINT)
		FMPR AC1,[3.0]
		MOVEM AC1,A33(JOINT)
		FSC AC1,1
		MOVEM AC1,A36(JOINT)
		HLRE AC1,-2(AC3)
		MOVEM AC1,A2(JOINT)
		FSC AC1,1
		MOVEM AC1,A22(JOINT)
		HRRE AC1,-2(AC3)
		MOVEM AC1,A1(JOINT)
		HLLZ AC1,-1(AC3)
		MOVEM AC1,SC(JOINT)
		HRRE AC1,-1(AC3)
		MOVEM AC1,A0(JOINT)
		JRST ELL]
ELL:	DIV T,TN(JOINT)
	MOVE AC1,A5(JOINT)
	MUL AC1,T
	ADD AC1,A4(JOINT)
	MUL AC1,T
	ADD AC1,A3(JOINT)
	MUL AC1,T
	ADD AC1,A2(JOINT)
	MUL AC1,T
	ADD AC1,A1(JOINT)
	MUL AC1,T
	ADD AC1,A0(JOINT)
	XOR AC1,SC(JOINT)
	FSC AC1,0
	MOVEM AC1,T0(JOINT)
	MOVE AC1,A55(JOINT)
	MUL AC1,T
	ADD AC1,A44(JOINT)
	MUL AC1,T
	ADD AC1,A33(JOINT)
	MUL AC1,T
	ADD AC1,A22(JOINT)
	MUL AC1,T
	ADD AC1,A1(JOINT)
	XOR AC1,SC(JOINT)
	FSC AC1,0
	MOVEM AC1,TDP(JOINT)
	MOVE AC1,A520(JOINT)
	MUL AC1,T
	ADD AC1,A412(JOINT)
	MUL AC1,T
	ADD AC1,A36(JOINT)
	MUL AC1,T
	ADD AC1,A22(JOINT)
	XOR AC1,SC(JOINT)
	FSC AC1,0
	MOVEM AC1,TDD(JOINT)
NE:	SOJGE I,EL
;This routine reads the real time clock and then computes the change in time
;from the last time in ac1.
;returns the period in micro sec in ac1

PERIOD:	CONI CLOCK,TAC1			;read the clock
	TLZ TAC1,777774			;zero out the hours minutes etc.
	SUBM TAC1,AC1			;compute change in time, leave it in ac1
	JRSTF (Q)			;if positive o.k.
	ADD AC1,[=1000000]		;otherwise add one second
	JRSTF (Q)


;This routine takes the three readings in ac=1 from the a/d
;it then computes the difference and if greater than MAXDEL returns without skiping
;otherwise it throws away the first and averages the second and third and skips 2

AVE:	LDB TAC1,[POINT 12,AC1,23]	;get second number in tac1
	HRRZI TAC2,(TAC1)		;and into tac2
	AND AC1,[7777]			;leave the third in ac1
	SUBI TAC2,(AC1)			;compute |diff|
	MOVM TAC2,TAC2
	CAIL TAC2,MAXDEL		;compare to maxdel
	JRSTF (Q)			;return without skip
	ADDI AC1,(TAC2)			;add together
	JRSTF 2(Q)			;skip 2

;this routine reads the a/d channel in ac1 and returns the three samples in ac1
;the three sign bits are complemented.
;if the a/d fails to respond it jumps to toff
	BEGIN PREAD
	DATMSD←TAC1			;number of tries to read a/d
	WAIT←TAC2			;wait loop for a/d
	DACVAL←TAC3			;where the a/d reading goes.

↑PREAD:	MOVEI DATMSD,3			;number of tries with data missed
DACRED:	SOJL DATMSD,[REDER:		;too many tries give up
		MOVEI TAC1,READER
		MOVEM TAC1,TRAJER
		JRST TOFF]
DACST:	CONO DB,4250			;set up the 136 for 3 12bit samples
	CONO AD,(AC1)			;start the a/d on channel in ac1
↑NREAD:	MOVEI WAIT,30			;jump here to wait for more samples
	CONSO DB,1000
	SOJGE WAIT,.-1
	JUMPL WAIT,[DACDIE:		;waited too long
		TRO REREAD		;turn on readread bit
		CONO AD,4000(AC1)	;stop the a/d
		MOVEI WAIT,12		;and wait for it to stop
	WRH:	SOJGE WAIT,WRH
		JRST DACRED]		;and try again
	DATAI DB,DACVAL			;read the data
	CONSZ DB,10000			;check for data missed
	JRST DACDIE			;if on go do it all again
	XOR DACVAL,SBMSK
	MOVE AC1,DACVAL			;return data
	JRST @READFN			;this allows a function to be performed
					;which if it jumps to NREAD will continue to get
					;the next three samples from this channel
↑READFN:FREAD

↑FREAD:	CONO AD,4000			;this is the default function which just
	JRSTF (Q)			;stops the a/d and returns

;takes motor torque in ac1 and drives motor (JOINT) if o.k.
;if drive excessive jumps to DHO
MOTOR:	FMPR AC1,KM(JOINT)			;convert to voltage

;now calculate back emf and add to supply voltage
	MOVE AC2,EMF(JOINT)
	FMPR AC2,TD(JOINT)			;back emf
	MOVSI TAC2,(30.0)			;supply voltage
	JUMPGE AC1,DRVLT
	MOVN AC2,AC2
DRVLT:	FSBR TAC2,AC2				;available drive voltage
	CAMGE TAC2,[1.0]
	JRST DHO				;no available drive voltage
VELOK:	MOVM AC2,AC1				;|reqd voltage|
	FDVR AC2,TAC2				;relative time on
	FIX AC2,211000				;1=16
	CAIL AC2,777000
	JRST DHO				;too much force stop the arm
	TRC AC2,400000				;complement the sign bit
	HRLI AC2,(JOINT)			;load the joint number
SETDRV:	DATAO WIDTH,AC2				;set the joint drive level
	MOVE TAC1,DATWD				;direction and go bits for yellow
	TDO TAC1,BMASK(JOINT)			;set for positive drive with brake off
	JUMPL AC1,ISN				;and if the drive is negative
	TDZ TAC1,DMASK(JOINT)			;set it for negative drive
ISP:	SKIPE REV(JOINT)			;this is true if the motor leads are reversed
	TDC TAC1,DMASK(JOINT)			;in which case we must complement the direction
	DATAO ARM,(TAC1)			;drive the joint
	MOVEM TAC1,DATWD			;and put datwd away
	JRSTF (Q)

;turns off the joint in JOINT

STOPJT:	MOVE TAC1,DATWD			;pick up yellow control word
	TDZ TAC1,BMASK(JOINT)		;turn on brake and go bit off
	DATAO ARM,(TAC1)		;do it!
	MOVEM TAC1,DATWD

	HRLZI TAC2,(JOINT)		;joint number in left half
	TRO TAC2,400000			;zero drive
	DATAO WIDTH,(TAC2)		;do it!

	JRSTF (Q)